Several control systems employed by vehicles, either autonomous vehicles or vehicles executing in autonomous-driving mode, predict future, safe motions, or paths, of the vehicle, both in order to avoid obstacles, such as other vehicles or pedestrians, but also to optimize some criteria associated to the operation of the vehicle. The vehicle is controlled along the path that is a part of the object trajectory or, more precisely, the projection of the trajectory onto the two-dimensional plane of movement. To say it more figuratively, the path is the line connecting all geometrical coordinates of the moving vehicle.
A fundamental objective of path planning is to construct a trajectory connecting two states, e.g., an initial state and a target state, of a moving object, such as autonomous or semi-autonomous vehicles executing in autonomous-driving mode, and to control the motion of the vehicles along the trajectory. A “state” as used herein is meant to be a vector including the geometrical coordinates of the vehicle in the plane and an orientation of the vehicle at these geometrical coordinates. The target state can either be a fixed location, a moving location, a velocity vector, a region, or a combination thereof. The surroundings, such as road edges, pedestrians, and other vehicles, are sensed by the sensors of the vehicle and/or are at least partially known by a priori given information.
A path connecting an initial state and a target state is normally made up of several elementary paths (or EPT for short). In some situations, the path planning solely works with elementary paths of the same or different kinds. To that end, the path of a moving body is normally made up of a plurality of elementary paths that share the same junction points (abbreviated as JP). Thus, the junction points are special points of the path because each of them belongs to two adjoining elementary paths (EPT).
One special characteristic demanded of the paths is that states do not change erratically along the entire course or at the junction points. This is to say that the transition from one elementary path to an adjacent elementary path must be ensured to be continuous at the junction points of the elementary paths as well as both its first derivative (tangent) and its second derivative (curvature). The path is continuous at junction points when the transition from one elementary path to the adjacent elementary path can be performed by changing a steering and velocity of the vehicle in a linear or continuous manner.
For example, pioneering work by Dubins and Reeds and Shepp investigated patterns of a shortest path between two states of the vehicle. The shortest path, termed as Reeds-Shepp's (RS) path, is a sequential composition of line segments and/or tangential circular arcs of a minimum turning radius. The RS path can be computed in a very efficient way, but the curvature along the RS path involving multiple segments is discontinuous. Such discontinuity of the curvature is undesirable in practice, since a vehicle has to stop and perform stationary steering, leading to unnecessary time delay and extra wearing of tires.
Some methods overcome discontinuity of the RS paths by introducing other segments, in addition to the line and circular segments. For example, the path planning can combine various types of elementary paths such as splines, polynomials, trigonometric functions or clothoids. The aforementioned elementary paths are mainly distinguished by their mathematical description. A feature that all elementary paths have in common is that they can be combined with straight and/or circular lines to form a continuous curvature path.
Some conventional methods prefer to base its path planning on clothoids because they are closely related to the rules of movement of steered motor vehicles. A clothoid is a special type of plane curves, which has its curvature, denoted by c, changing in a linear manner along the path. Clothoids have the added benefit of the length of the path being minimized when an initial state is connected with a terminal state.
For example, the teaching of T. Fraichard and A. Scheuer, “From Reeds and Shepp's to continuous curvature paths,” introduces a concept of a clothoid turn (abbreviated as CC turn) formed by two clothoids connected by a circular arc, and teaches μ-tangency conditions to ensure continuous curvature transitions among CC turns and straight lines. However, a continuous path typically includes a sequence of multiple CC turns and lines, and it can be time consuming to find the parameters of the optimal sequence forming the continuous path.
For example, method described in U.S. 2016/03133735 first determines an RS path and tries to find a continuous curvature path which stays as close as possible to the RS path. This method computes the continuous curvature path by solving optimization problems with nonlinear constraints. However, the solution to this method requires iterative process, which can be computationally prohibitive for a number of practical applications. Also, the resultant optimization problem of that method is non-convex. A feasible solution to the non-convex problem is not guaranteed solved by the iterative process, and thus the construction of the continuous curvature path may fail. Hence, this method may not be suitable for a real-time control of the vehicles.
Similarly, the method described in U.S. Pat. No. 8,428,820 improves the computation efficiency, but still entails computationally expensive iterative process to construct the continuous curvature path. Hence, this method also may not be suitable for a real time control of the vehicles.
Accordingly, there is a need to construct a continuous curvature path for controlling a motion of a vehicle from an initial to a target state in a manner suitable for real-time control of the vehicles.